#include "Include.h"

void CMP_OPA_Init()
{

    // ENABLE CMP AND OPA
    _c0en=1;
    _c1en=1;
    _c2en=1;
    _c3en=1;
    _c4en=1;    
    _c5en=1;     
    _open=1;

	_c5hyson1 = 0;	_c5hyson0 = 0;
	_c4hyson1 = 0;	_c4hyson0 = 0;
    _c3hyson1 = 0;	_c3hyson0 = 0;
    _c2hyson1 = 0;	_c2hyson0 = 0;
    _c1hyson1 = 0;	_c1hyson0 = 0;
    _c0hyson1 = 0;	_c0hyson0 = 0;
	
	Calibration_CMP0();
    Calibration_OPA();
    Calibration_CMP(CMP1C0_SECTOR,CMP1C0_ADDR);
    Calibration_CMP(CMP2C0_SECTOR,CMP2C0_ADDR);
    Calibration_CMP(CMP3C0_SECTOR,CMP3C0_ADDR);
    Calibration_CMP(CMP4C0_SECTOR,CMP4C0_ADDR);
    Calibration_CMP(CMP5C0_SECTOR,CMP5C0_ADDR);

    //----------OPA------------------------   
    _opc = 0b10000000;	//enable opa
    _ops = 0b00010000;  // Set OPA internal channel	0b00000110;  ? ? ? OPS4 OPS3 OPS2 OPS1 OPS0
    _opoofm=0;
    //s4 s3 s2 s1 s0

	//-----------CMP--------------------- 
	_cmpctl0 = 0b11000100;	
	_cmp0db = 0b10000100;	//fDBDL=fsys/2	

	//---------CMP0--------
	_cmp0c1 = 0b10000111;
	 
	//---------CMP1--------
	//_c1da = 160;	//1140
	_c1da = 163;//TEST 1160v
	//_c1da = 147;//TEST 1050
	_cmp1c1 = 0b10000001;//debounce time:3-4 tDBDL	
			
	//---------CMP2--------
	_c2da = 179;	
	_cmp2c1 = 0b10000001;//debounce time:3-4 tDBDL
		
	//---------CMP3--------
	_c3da = 15;	//290mv
	_c3lebc = 0b11010111;//44/fsys~46/fsys
	_cmp3c1 = 0b10000001;//debounce time:3-4 tDBDL		
	_c3leben =0;
	

    //----------- CMP4-----------------------
	_c4da = 95;	

	_cmp4c1 = 0b10001111;//debounce 16us
 	 	
    //----------- CMP5-----------------------
 	_cmpctl1 = 0b00000011;//CP5N
 	_c5da = 85;	
 	_cmp5c1 = 0b10000010;//debounce time:7-8 tDBDL	
 	
	_c0en=1; 
  	_c1en=1;	 
    _c2en=1;
    _c3en=0;
    _c4en=1;
    _c5en=1;
}

void ISR_CMP_OPA_Init()
{
    _cp1e = 1; // Comparator 1 interrupt enable
    _cp1f = 0; // Comparator 1 interrupt request bit is cleared
    _cp2e = 1; // Comparator 2 interrupt enable
    _cp2f = 0; // Comparator 2 interrupt request bit is cleared
    _cp3e = 1; // Comparator 3 interrupt enable
    _cp3f = 0; // Comparator 3 interrupt request bit is cleared
    _cp4e = 1; // Comparator 4 interrupt enable
    _cp4f = 0; // Comparator 4 interrupt request bit is cleared
    _cp5e = 1; // Comparator 5 interrupt enable
    _cp5f = 0; // Comparator 5 interrupt request bit is cleared
    _rlbf = 0; // PPG Reload from PPGTA
}

void Calibration_OPA()
{
    vu8 i, tmp1, tmp2;
    _opocal = 0B11000000;
    GCC_DELAY(2000); //

    tmp1 = _opo; // Enter calibration mode
    for (i = 0; i < 63; i++)
    {
        _opocal++;
        GCC_DELAY(2000);
        GCC_CLRWDT();

        GCC_CLRWDT();
        if (_opo != tmp1)
        {
            GCC_NOP();
            break;
        }
    }
    tmp2 = i;

    _opocal = 0B11111111; // Enter calibration mode
    GCC_DELAY(2000); //
    
    tmp1 = _opo;
    for (i = 63; i > 0; i--)
    {
        _opocal--;
        GCC_DELAY(2000);
        GCC_CLRWDT();
        if (_opo != tmp1)
        {
            GCC_NOP();
            break;
        }
    }
    tmp2 = tmp2 + i;
    tmp2 = tmp2 >> 1;
    _opocal = 0b00000000;
    _opocal = _opocal + tmp2; // Exit calibration mode
}

void Calibration_CMP0()
{
    vu8 i, tmp1, tmp2;
    _cmp0c0 = 0b01000000;
    _c0crs1 = 0; 	_c0crs0 = 1;
    
    GCC_DELAY(1000);

    tmp1 = _c0cmpop; // Enter calibration mode
    for (i = 0; i < 63; i++)
    {
		_cmp0c0++;
        GCC_DELAY(1000);
        GCC_CLRWDT( );
        if (_c0cmpop != tmp1)
        {
            GCC_NOP();
            break;
        }
    }
    tmp2 = i;

	_cmp0c0 = 0b01111111;
	_c0crs1 = 0; 	_c0crs0 = 1;
	GCC_DELAY(1000);	
	
	tmp1=_c0cmpop;				
	for(i=63;i>0;i--)
	{	
		_cmp0c0--;		
		GCC_DELAY(1000);
		GCC_CLRWDT();
		if(_c0cmpop!=tmp1)
		{
			GCC_NOP();
			break;
		}			
	}
	tmp2=tmp2+i;
	tmp2=tmp2>>1;
	_cmp0c0 = 0b00000000;
	
	_cmp0c0=_cmp0c0+tmp2;	
}



void Calibration_CMP(u8 sector, u8 addr)
{
    vu8 i, tmp1, tmp2;
    _mp1h = sector;
    _mp1l = addr;
    _iar1 = 0B01100000;
    GCC_DELAY(1000);

    tmp1 = _iar1 & 0x80; // Enter calibration mode
    for (i = 0; i < 31; i++)
    {
        _iar1++;
        GCC_DELAY(1000);
        GCC_CLRWDT();
        if ((_iar1 & 0x80) != tmp1)
        {
            GCC_NOP();
            break;
        }
    }
    tmp2 = i;

    _iar1 = 0B01111111; // Enter calibration mode
    GCC_DELAY(1000);
    tmp1 = _iar1 & 0x80;
    for (i = 31; i > 0; i--)
    {
        _iar1--;
        GCC_DELAY(1000);
        GCC_CLRWDT();
        if ((_iar1 & 0x80) != tmp1)
        {
            GCC_NOP();
            break;
        }
    }
    tmp2 = tmp2 + i;
    tmp2 = tmp2 >> 1;
    _iar1 = 0b10000000;
    _iar1 = _iar1 + tmp2; // Exit calibration mode
}
